/*
 *  Implementation of the BMA180Controller class.
 *
 */

#include "WProgram.h"
#include "BMA180Controller.h"
#include <string.h> //memcpy
#include <wiring.h> // for boolean and byte
#include <Wire.h>
#undef round 

//Address & mask defines for BMA180//

//Chip ID & Version registers
#define CHIP_ID_ADDR 0x00
  #define CHIP_ID_VALUE 0x03
#define VERSION_ADDR 0x01

// Data registers
#define ACCEL_X_LSB_ADDR 0x02
  #define NEW_DATA_MASK 0x01
#define ACCEL_X_MSB_ADDR 0x03
#define ACCEL_Y_LSB_ADDR 0x04
//  #define NEW_DATA_MASK 0x01
#define ACCEL_Y_MSB_ADDR 0x05
#define ACCEL_Z_LSB_ADDR 0x06
//  #define NEW_DATA_MASK 0x01
#define ACCEL_Z_MSB_ADDR 0x07
#define TEMPERATURE_ADDR 0x08

// Status registers
#define STATREG1_ADDR 0x09
#define STATREG2_ADDR 0x0A
#define STATREG3_ADDR 0x0B
#define STATREG4_ADDR 0x0C

// Control registers
#define CTRLREG0_ADDR 0x0D
  #define EEPROM_WRITE_MASK 0x10 
  #define RESET_INT_MASK 0x40
  #define SLEEP_MASK 0x02
  #define DISABLE_WAKEUP_MASK 0x01

#define CTRLREG1_ADDR 0x0E
  #define EN_OFFSET_X_MASK 0x80
  #define EN_OFFSET_Y_MASK 0x40
  #define EN_OFFSET_Z_MASK 0x20

#define CTRLREG2_ADDR 0x0F
  #define UNLOCK_EE_MASK 0xF0

#define SOFT_RESET_ADDR 0x10

#define BWTCS_ADDR 0x20
  #define BANDWIDTH_MASK 0xF0
    #define BANDWIDTH_10HZ 0x00
    #define BANDWIDTH_20HZ 0x10
    #define BANDWIDTH_40HZ 0x20
    #define BANDWIDTH_75HZ 0x30
    #define BANDWIDTH_150HZ 0x40
    #define BANDWIDTH_300HZ 0x50
    #define BANDWIDTH_600HZ 0x60
    #define BANDWIDTH_1200HZ 0x70

#define CTRLREG3_ADDR 0x21
  #define SLOPE_ALERT_MASK 0x80
  #define SLOPE_INT_MASK 0x40
  #define HIGH_INT_MASK 0x20
  #define LOW_INT_MASK 0x10
  #define TAPSENSE_INT_MASK 0x08
  #define ADV_INT_MASK 0x04
  #define NEW_DATA_INT_MASK 0x02
  #define LATCH_INT_MASK 0x01

#define TCO_Y_ADDR 0x2F
  #define WAKEUP_DUR_MASK 0x03
    #define WAKEUP_DUR_20MS 0x00
    #define WAKEUP_DUR_80MS 0x01
    #define WAKEUP_DUR_320MS 0x02
    #define WAKEUP_DUR_2560MS 0x03
  
#define TCO_Z_ADDR 0x30
  #define MODE_CONFIG_MASK 0x03
    #define MODE_CONFIG_LOW_NOISE 0x00
    #define MODE_CONFIG_ULTRA_LOW_NOISE 0x01
    #define MODE_CONFIG_LOW_NOISE_RED_PWR 0x02
    #define MODE_CONFIG_LOW_PWR 0x03

#define GAIN_Z_ADDR 0x34
  #define WAKE_UP_MASK 0x01

#define OFFSET_LSB1_ADDR 0x35
  #define RANGE_MASK 0x0E
    #define RANGE_1G       0x00 //000
    #define RANGE_1G_LSB   0.13 // mg/LSB
    #define RANGE_1_5G     0x02 //001
    #define RANGE_1_5G_LSB 0.19 // mg/LSB
    #define RANGE_2G       0x04 //010
    #define RANGE_2G_LSB   0.25 // mg/LSB
    #define RANGE_3G       0x06 //011
    #define RANGE_3G_LSB   0.38 // mg/LSB
    #define RANGE_4G       0x08 //100
    #define RANGE_4G_LSB   0.50 // mg/LSB
    #define RANGE_8G       0x0A //101
    #define RANGE_8G_LSB   0.99 // mg/LSB
    #define RANGE_16G      0x0C //110
    #define RANGE_16G_LSB  1.98 // mg/LSB
  #define OFFSET_X_MASK 0xF0
  
#define OFFSET_LSB1_ADDR 0x36
  #define OFFSET_Y_MASK 0x0F
  #define OFFSET_Z_MASK 0xF0


// I2C addresses of the BMA180 devices.
#define BMA180_ADDR1 0x40
#define BMA180_ADDR2 0x41

BMA180Controller::BMA180Controller()
    : address(BMA180_ADDR1)
    , initialized(false)
    , range_lsb(0)
    , last_read_time(0)
{
  data_set.accel_x=0.0;
  data_set.accel_y=0.0;
  data_set.accel_z=0.0;
  data_set.time=0;
}

BMA180Controller::ResultType BMA180Controller::init()
{
  byte data=0;
  byte result=0;
  byte wait_count=0;
  
  // the user must wait 10 seconds before first write and
  // between each EEPROM write
  delay(10);
             
  // start a transmission to the device
  Wire.beginTransmission(address);
  // check the version by sending the Chip ID address
  Wire.send(CHIP_ID_ADDR);
  // and requesting the contents of that byte
  Wire.requestFrom(address, (byte)1 /*byte*/);
  
  // wait for the device to respond
  do {
    delay(1); 
    wait_count++;
  } while (0 == Wire.available() && wait_count < 100);
  
 
  // now that the chip has responded, get data it sent
  if (100 != wait_count)
  {
    data=Wire.receive();
  }
  // terminate the transmission, we're done with our first
  // command
  result=Wire.endTransmission();
  
  // if the chip matches what we expected and the command
  // terminated successfully, move on with initialization.
  if ( CHIP_ID_VALUE == data && 0 == result && wait_count < 100)
  {
    // start our next command, but insert a delay because each
    // write requires a 10 ms delay.
    delay(10); 
    Wire.beginTransmission(address);
    // enable control registers by writting a "1" to the
    // EEPROM_Write enable bit
    Wire.send(CTRLREG0_ADDR);
    Wire.send(EEPROM_WRITE_MASK);
    result=Wire.endTransmission();
    // if the result was bad, get out log the error
    if (0 != result) 
    {
      // start our next command
      delay(10); 
      Wire.beginTransmission(address);
      // set up bandwidth
      Wire.send(BWTCS_ADDR);
      Wire.send(BANDWIDTH_10HZ);
      result=Wire.endTransmission();
    }
    
    // if the result was bad, get out log the error
    if (0 != result) 
    {
      // start our next command
      delay(10); 
      Wire.beginTransmission(address);
      // set up the range
      Wire.send(OFFSET_LSB1_ADDR);
      Wire.send(RANGE_2G); 
      result=Wire.endTransmission();
      // if the result was bad, get out log the error
    }
    
    if (0 != result)
    {
      this->range_lsb=RANGE_2G_LSB;
    
    

/*  We'll pick this up when we go to use the interrupts

      // start our next command
      delay(10); 
      Wire.beginTransmission(address);
      // set up interrupts
      Wire.send(CTRLREG3_ADDR);
      Wire.send(NEW_DATA_INT_MASK); // if we wanted to enable multiple
                                    // interrupts here, they would be
                                    // "or"ed together, ie (A|B|C|D)
      result=Wire.endTransmission();]
    }
      // if the result was bad, get out log the error
    if (0 != result)
    {
*/
    
      // everything succeeded, return successful result
      // and bypass error logs below
      this->initialized=true;
      this->last_read_time=millis();
      return SUCCESS;
    }
  }


  // if we got here, then we hit some sort of error connecting.
  Serial.println("Error connecting to BMA180:");
  Serial.print("\ttransmission result = "); Serial.println(result);
  Serial.print("\twait_count = "); Serial.println(wait_count, DEC);

  return FAILURE;
}

byte BMA180Controller::getAByte(
  byte reg_addr,
  byte &data)
{
  // request the 1 byte data register from our device  
  Wire.beginTransmission(address);
  Wire.send(reg_addr);
  Wire.requestFrom(address,(byte)1);
  // wait for our data
  while( 0 == Wire.available() );
  
  data=Wire.receive();
  return Wire.endTransmission();
  
}

byte BMA180Controller::getAcceleration(
            byte reg_lsb_addr, 
            byte reg_msb_addr, 
            boolean & new_data, 
            float & acceleration)
{
  byte lsb_data;
  byte msb_data;
  short raw_accel_data = 0;
  byte result;
  
  new_data=false;
  raw_accel_data=0;
    
  // find out if data is available.  this is done by
  // reading the first bit out of the LSB of the acceleration
  // data registers.
  if( 0 != (result=this->getAByte(reg_lsb_addr,lsb_data)) )
  {
    // This if-check has a lot going on, but basically, we're
    // calling the routine and storing it's results in "result".
    // Because the assignment operator ("=") returns a reference
    // to the variable, we can just check the variable against 
    // our value (zero) to see if all is well.
    // If we have an error, then we'll kick out of this processing
    // and pick up outside the if (this->initialized) scope and 
    // log an error.
    return result;
  }
  
  // if we don't have dat to read, get out;
  if( 0 == ( new_data = (lsb_data & NEW_DATA_MASK) ) )
  {
    // this isn't an error like the statement above... we just don't
    // have data to store.  result here will return a good status, but
    // when the user checks new_data, it'll be false.
    return result;
  }
  else
  {
    // since the mask indicated we have data, we need to hold that
    // and get our upper byte of data

    if( 0 != (result=this->getAByte(reg_msb_addr,msb_data)) )
    {
      return result;
    }
      
    // if we're here, then the retrieval succeeded and we can put the
    // new data into our storage variable. 
    // First, we'll start with the MSB data.
    raw_accel_data=msb_data;
    // then, because the MSB is in the upper 8 bits, we need to shift
    // raw_accel to the left.
    raw_accel_data = (raw_accel_data << 8);
    // now let's put the LSB data in
    raw_accel_data |= (lsb_data);
    
    // now that we have all the data from the two registers, we need to
    // strip off the last 2 bytes because of the NEW_DATA indicator and
    // an extra bit of buffer (we've only got 14 bits of precision)
    raw_accel_data = (raw_accel_data >> 2);
      
  }
  
  
  // convert our fixed-point raw data to floating point "g"s
  acceleration = (float)raw_accel_data / (1000*this->range_lsb);
  
  return result;
}


BMA180Controller::ResultType BMA180Controller::readData()
{

  boolean new_data = false;
  float temp_accel = 0.0;
  byte result;

  AccelDataSet temp;
  
  // if we haven't connected, then we can't read anythign
  if( this->initialized )
  {
  
    // let's get the acceleration from the sensor usring our
    // getAcceleration routine
    if( 0 == (result=this->getAcceleration( ACCEL_X_LSB_ADDR, ACCEL_X_MSB_ADDR, 
                                            new_data, temp_accel)) )
    {
       
      if( new_data )
      {
        // if we successfully retrieved new data, store it in temp
        temp.accel_x = temp_accel;
      }
      else
      {
        // don't store the data and get out
        return SUCCESS;
      }
    }
    
    // Now, let's do it again for Y and Z axes.
    if( 0 == result && 0 == (result=this->getAcceleration( ACCEL_Y_LSB_ADDR, ACCEL_Y_MSB_ADDR, 
                                            new_data, temp_accel)) )
    {

       
      if( new_data )
      {
        // if we successfully retrieved new data, store it in temp
        temp.accel_y = temp_accel;
      }
      else
      {
        // don't store the data and get out
        return SUCCESS;
      }
    }
  
    // Now, let's do it again for Y and Z axes.
    if( 0 == result && 0 == (result=this->getAcceleration( ACCEL_Z_LSB_ADDR, ACCEL_Z_MSB_ADDR, 
                                            new_data, temp_accel)) )
    {
       
      if( new_data )
      {
        // if we successfully retrieved new data, store it in temp
        temp.accel_z = temp_accel;
      }
      else
      {
        // don't store the data and get out
        return SUCCESS;
      }

      // Store off the last read time.  This may not be useful
      // in this example, but it may be useful in the future.
      this->last_read_time=data_set.time;  
      // get the new time to store in teh data_set
      data_set.time=millis();
      // copy the temporary data in to our class member
      memcpy(&data_set, &temp, sizeof(AccelDataSet));
      
      return SUCCESS;
    }
  } 
  
  // if we got here, then we hit some sort of error connecting.
  Serial.println("Error getting data from the BMA180:");
  Serial.print("\tinitialized = "); Serial.println(this->initialized);
  Serial.print("\ttransmission result = "); Serial.println(result);

  return FAILURE;
}

BMA180Controller::AccelDataSet BMA180Controller::getData()
{
   return data_set; 
}

